﻿#include "stdafx.h"
#include "MathTools.h"

_NAMESPACE_BEGIN

void MathTools::Quat2Matrix( CONST_REF(Quaternion)quat, Matrix3& mat )
{
	double length2 = quat.w*quat.w + quat.x*quat.x + quat.y*quat.y + quat.z*quat.z;

	if (fabs(length2) <= std::numeric_limits<double>::min())
	{
		mat[0][0] = 0.0; mat[1][0] = 0.0; mat[2][0] = 0.0;
		mat[0][1] = 0.0; mat[1][1] = 0.0; mat[2][1] = 0.0;
		mat[0][2] = 0.0; mat[1][2] = 0.0; mat[2][2] = 0.0;
	}
	else
	{
		double rlength2;
		if (length2 != 1.0)
		{
			rlength2 = 2.0/length2;
		}
		else
		{
			rlength2 = 2.0;
		}

		double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

		x2 = rlength2*quat.x;
		y2 = rlength2*quat.y;
		z2 = rlength2*quat.z;

		xx = quat.x * x2;
		xy = quat.x * y2;
		xz = quat.x * z2;

		yy = quat.y * y2;
		yz = quat.y * z2;
		zz = quat.y * z2;

		wx = quat.w * x2;
		wy = quat.w * y2;
		wz = quat.w * z2;

		mat[0][0] = 1.0 - (yy + zz);
		mat[1][0] = xy - wz;
		mat[2][0] = xz + wy;


		mat[0][1] = xy + wz;
		mat[1][1] = 1.0 - (xx + zz);
		mat[2][1] = yz - wx;

		mat[0][2] = xz - wy;
		mat[1][2] = yz + wx;
		mat[2][2] = 1.0 - (xx + yy);
	}
}

void MathTools::Matrix2Quat( CONST_REF(Matrix3) mat, Quaternion&quat )
{
	float s;
	float tq[4];
	int    i, j;

	tq[0] = 1 + mat[0][0]+mat[1][1]+mat[2][2];
	tq[1] = 1 + mat[0][0]-mat[1][1]-mat[2][2];
	tq[2] = 1 - mat[0][0]+mat[1][1]-mat[2][2];
	tq[3] = 1 - mat[0][0]-mat[1][1]+mat[2][2];

	j = 0;
	for(i=1;i<4;i++) j = (tq[i]>tq[j])? i : j;

	if (j==0)
	{
		quat.w = tq[0];
		quat.x = mat[1][2]-mat[2][1];
		quat.y = mat[2][0]-mat[0][2];
		quat.z = mat[0][1]-mat[1][0];
	}
	else if (j==1)
	{
		quat.w = mat[1][2]-mat[2][1];
		quat.x = tq[1];
		quat.y = mat[0][1]+mat[1][0];
		quat.z = mat[2][0]+mat[0][2];
	}
	else if (j==2)
	{
		quat.w = mat[2][0]-mat[0][2];
		quat.x = mat[0][1]+mat[1][0];
		quat.y = tq[2];
		quat.z = mat[1][2]+mat[2][1];
	}
	else /* if (j==3) */
	{
		quat.w = mat[0][1]-mat[1][0];
		quat.x = mat[2][0]+mat[0][2];
		quat.y = mat[1][2]+mat[2][1];
		quat.z = tq[3];
	}

	s = sqrt(0.25/tq[j]);
	quat.w *= s;
	quat.x *= s;
	quat.y *= s;
	quat.z *= s;
}

MathTools::MathTools()
{

}

MathTools::~MathTools()
{

}

_NAMESPACE_END